﻿using System;
using System.Collections;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Presentation;
using Microsoft.SPOT.Presentation.Controls;
using Microsoft.SPOT.Presentation.Media;
using Microsoft.SPOT.Presentation.Shapes;
using Microsoft.SPOT.Touch;

using Gadgeteer.Networking;
using GT = Gadgeteer;
using GTM = Gadgeteer.Modules;
using Gadgeteer.Modules.GHIElectronics;
using MLRobotic.Asserv.Drivers;
using MLRobotic.Asserv.Utils;

namespace MLRobotic.Asserv
{
    public partial class Program
    {
        private ILogger _log = Log.Instance;
        private Motion _motion;
        private Motors _motors;
        private Thread _asservThread;
        private SpiHub _spiHub;
        private PulseCount _pulseCountLeft;
        private PulseCount _pulseCountRight;
        private Com _com;
        private Button _startButton;
        private Button _stopButton;
        private bool _isEmergencyStopped;

        // debug asserv facility
        private int _phase = 0;

        // This method is run when the mainboard is powered up or reset.   
        private void ProgramStarted()
        {
            Debug.Print("Program Started");
            Init();
        }

        private void Init()
        {
            _log.IsEnabled = false;
            //_log.IsEnabled = System.Diagnostics.Debugger.IsAttached;

            _motion = new Motion();
            _motors = new Motors(motorController);

            // modules
            _com = new Com(2, _motion);
            _startButton = new Button(3);
            _spiHub = new SpiHub(5);
            _pulseCountLeft = new PulseCount(_spiHub.SHubSocket2);
            _pulseCountRight = new PulseCount(_spiHub.SHubSocket1);
            _stopButton = new Button(7);

            // events
            _startButton.ButtonPressed += startButton_ButtonPressed;
            _stopButton.ButtonPressed += (s, e) => Emergency();
            _motion.MotionTerminated += MotionTerminated;
            _motion.TranslationStarted += (s, e) => _com.SendTranslationStarted();

            // inits
            _motion.InitOdometry(ReadLeft(), ReadRight());
            _motion.SetPosition(0, 0, 0);

            _pulseCountLeft.ResetCounter();
            _pulseCountRight.ResetCounter();

            ReleaseEmergency();

            // main loop
            _asservThread = new Thread(new ThreadStart(MainThread));
            _asservThread.Start();
        }

        private void startButton_ButtonPressed(Button sender, Button.ButtonState state)
        {
            ReleaseEmergency();
            //_phase = _phase == 0 ? 1 : 2;
            DebugSequence();
        }

        private void Emergency()
        {
            _isEmergencyStopped = true;
            _startButton.TurnLEDOff();
            _stopButton.TurnLEDOn();
            _phase = 0;
        }

        private void ReleaseEmergency()
        {
            _isEmergencyStopped = false;
            _startButton.TurnLEDOn();
            _stopButton.TurnLEDOff();
        }

        private void MotionTerminated(object sender, Microsoft.SPOT.EventArgs args)
        {
            _com.SendMoveFinished();
            DebugSequence();
        }

        private void DebugSequence()
        {
            if (_phase == 0)
                return;
            
            switch (_phase)
            {
                case 0:
                case 1:
                    break;
                case 2:
                    Debug.Print("GOTO 0");
                    _motion.MoveTo(0, 0);
                    _phase++;
                    break;
                case 3:
                    Debug.Print("AVANCE 1000");
                    _motion.MoveTo(1000, 0);
                    _phase++;
                    break;
                case 4:
                    Debug.Print("TOURNE 30");
                    _motion.Turn(30);
                    _phase = 1;
                    break;
                case 5:
                    Debug.Print("RECULE 100");
                    _motion.Move(-100);
                    _phase++;
                    break;
                case 6:
                    Debug.Print("TOURNE -120");
                    _motion.Turn(-120);
                    _phase = 1;
                    break;
                case 7:
                    _motion.MoveTo(0, 0);
                    _phase++;
                    break;
                case 8:
                    _motion.TurnTo(0);
                    _phase++;
                    break;
                case 9:
                    _log.Flush();
                    _phase++;
                    break;
                default:
                    _phase = 0;
                    break;
            }
        }

        private void MainThread()
        {
            while (true)
            {
                DateTime dueDate = DateTime.Now.AddMilliseconds(_motion.Te);

                // asserv
                int leftPwm, rightPwm;

                int left = ReadLeft();
                int right = ReadRight();

                _motion.Process(left, right, out leftPwm, out rightPwm);

                // move
                if (!_isEmergencyStopped)
                {
                    _motors.MoveLeft(leftPwm);
                    _motors.MoveRight(rightPwm);
                }
                else
                {
                    _motors.MoveLeft(0);
                    _motors.MoveRight(0);
                }

                // TEST PURPOSE: Turn Led On when saturating pwm
                //if ((leftPwm > 95 && rightPwm > 95) ||(leftPwm < -95 && rightPwm < -95))
                //    saturationButton.TurnLEDOn();
                //else
                //    saturationButton.TurnLEDOff();

                // com
                _com.Process();

                // loop timing
                int wait = dueDate.Subtract(DateTime.Now).Milliseconds;
                if (wait > 0)
                    Thread.Sleep(wait);
                //else
                //    Debug.Print(wait.ToString());
            }
        }

        private int ReadLeft()
        {
            return _pulseCountLeft.ReadCounter();
        }

        private int ReadRight()
        {
            return -_pulseCountRight.ReadCounter();
        }
    }
}
